/**
 * @file spider_leg.h
 * @author SunWater
 * @brief 蜘蛛机器人腿部算法解算
 * 此蜘蛛腿部算法解算是基于三角函数的正向运动学和反向运动学
 * 第一节腿的起始位置为原点
 * 第一节腿只有yaw，第二节腿只有pitch，第三节腿只有pitch
 * @version 0.1
 * @date 2025-04-06
 * 
 */

#ifndef __SPIDER_LEG_H
#define __SPIDER_LEG_H

// c++内容
#ifdef __cplusplus

#endif

// c语言内容
#ifdef __cplusplus
extern "C" {
#endif

// 腿的相关参数(右手坐标系, 以第一节腿的起始为原点)
typedef struct {
    // 腿长(mm)
    float leg_length;

    // 腿的初始角度
    float start_angle_yaw;    // 起始角度，如果单位不是rad，在之后set_pos来连接腿时，开启deg_mode
    float start_angle_pitch;  // 起始角度，如果单位不是rad，在之后set_pos来连接腿时，开启deg_mode
    float start_angle_roll;   // 起始角度，如果单位不是rad，在之后set_pos来连接腿时，开启deg_mode
    
    // 腿的初始位置
    float start_x;  // 初始位置x(mm)
    float start_y;  // 初始位置y(mm)
    float start_z;  // 初始位置z(mm)

    // 腿的结束位置
    float end_x;  // 初始位置x(mm)
    float end_y;  // 初始位置y(mm)
    float end_z;  // 初始位置z(mm)

} Leg;

// 腿的运动学状态
typedef enum {
    LEG_IK_OK = 0,  // 正常

    D_COS_OUT_OF_RANGE = 1,    // D_cos 超出范围
    D_1_COS_OUT_OF_RANGE = 2,  // D_1_cos 超出范围
    D_2_COS_OUT_OF_RANGE = 3,  // D_2_cos 超出范围
} Leg_IK_Status;

/**
 * @brief 设置腿的位置
 * 
 * @param leg_1 第一节腿
 * @param leg_2 第二节腿
 * @param leg_3 第三节腿
 * @param yaw 第一节腿的角度
 * @param pitch 第二节腿的角度 
 * @param roll 第三节腿的角度
 * @param deg_mode 输入角度的单位为度
 */
void Leg_set_pos(Leg *leg_1, Leg *leg_2, Leg *leg_3,
                 float yaw, float pitch, float roll, _Bool deg_mode);

/**
 * @brief 正向运动学计算
 *
 * @param theta1 第一节腿的角度(yaw)
 * @param theta2 第二节腿的角度(pitch)
 * @param theta3 第三节腿的角度(pitch)
 * @param L1 第一节腿的长度
 * @param L2 第二节腿的长度
 * @param L3 第三节腿的长度
 * @param output_array 输出数组，长度为12
 * @param output_array[0:2] 为第一节腿的原点x,y,z
 * @param output_array[3:5] 为第二节腿的起始位置x,y,z
 * @param output_array[6:8] 为第三节腿的起始位置x,y,z
 * @param output_array[9:11] 为第三节腿的结束位置x,y,z
 */
void Leg_forward_kinematics(float theta1, float theta2, float theta3, float L1,
                            float L2, float L3, float *output_array);

/**
 * @brief 反向运动学计算
 * 使用了推导出的妙妙小公式
 *
 * @param x 末端目标位置x
 * @param y 末端目标位置y
 * @param z 末端目标位置z
 * @param L1 第一节腿的长度
 * @param L2 第二节腿的长度
 * @param L3 第三节腿的长度
 * @param output_array 输出数组，长度为3
 * @return Leg_IK_Status 反向运动学状态
 */
Leg_IK_Status Leg_inverse_kinematics(float x, float y, float z, float L1,
                                     float L2, float L3, float *output_array);
#ifdef __cplusplus
}
#endif

#endif